// Author of SSL_SLAM2: Wang Han 
// Email wh200720041@gmail.com
// Homepage https://wanghan.pro

//c++ lib
#include <cmath>
#include <vector>
#include <chrono>

//ros lib
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/synchronizer.h>

class mapOptimizationNode : public rclcpp::Node 
{

public:

message_filters::Subscriber<nav_msgs::msg::Odometry> subOdometry1;
message_filters::Subscriber<nav_msgs::msg::Odometry> subOdometry2;

typedef message_filters::sync_policies::ApproximateTime<nav_msgs::msg::Odometry, nav_msgs::msg::Odometry> cloudodomaprox_policy;

mapOptimizationNode() : Node("mapOptimizationNode"){

    // Message filters subscribe to both PointCloud2 topics and Odometry topic
    subOdometry1.subscribe(this, "/odom");
    subOdometry2.subscribe(this, "/light_odom"); 
	
    static message_filters::Synchronizer<cloudodomaprox_policy> syncApproximate(5, subOdometry1, subOdometry2);
    syncApproximate.registerCallback(std::bind(&mapOptimizationNode::velodyneCloudOdomSyncHandler, this, std::placeholders::_1, std::placeholders::_2));
    RCLCPP_INFO(this->get_logger(),"map Optimization node started");
}

void velodyneCloudOdomSyncHandler(const nav_msgs::msg::Odometry::ConstSharedPtr& odom1Msg,
								  const nav_msgs::msg::Odometry::ConstSharedPtr& odom2Msg){
    RCLCPP_INFO(this->get_logger(), "odom1: %f odom2: %f", odom1Msg->twist.twist.angular.x, odom2Msg->pose.pose.position.x/64705.5);
}
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto mON {std::make_shared<mapOptimizationNode>()};
    rclcpp::spin(mON);
    rclcpp::shutdown();

    return 0;
}